#include "dshot.h"

#include <stdint.h>
#include <stdlib.h>
#include <string.h>

#include <usdev/pwm.h>

#define ESC_CMD_BUFFER_LEN 16
#define ESC_BIT_0 62
#define ESC_BIT_1 124

using namespace NXMFC;

dshot::dshot(/* args */)
{
    buf = 0;
}

dshot::~dshot()
{
}

bool dshot::init(const char *dev, enum dshot_type type, int channels_need)
{
    struct pwm_caps cap;
    struct pwm_configuration cfg;
    struct pwm_reqbufs reqb;

    if (!open(dev))
        return false;

    if (ioctl(PWM_IOC_QUERYCAP, &cap) != 0)
    {
        return false;
    }

    if (cap.num_channels != channels_need) 
    {
        return false;
    }

    if (!(cap.flags & PWM_CF_QUEUED_PULSE))
    {
        return false;
    }

    cfg.freq_hz = type;
    cfg.iomode = PWM_MODE_OUTPUT_QUEUED_PULSE;
    cfg.polarity = PWM_POLARITY_NORMAL;
    cfg.pulse_ns = 0;

    if (ioctl(PWM_IOC_SCONFIG, &cfg) != 0)
    {
        return false;
    }

    reqb.buf_count = 1;
    reqb.pulse_count_per_channel = 18;
    reqb.type = PWM_BUFTYPE_RAW;

    if (ioctl(PWM_IOC_REQBUFS, &reqb) != 0)
    {
        return false;
    }

    buf = (unsigned short*)malloc(4 * 18 * 2);
    if (!buf)
    {
        return false;
    }
    memset(buf, 0, 4 * 18 * 2);

    return true;
}

int dshot::output(unsigned short value[4])
{
    int ret;
    uint16_t data[4];

    for (int i = 0; i < 4; i ++)
        data[i] = add_checksum(value[i]);

    for (int i = 0; i < ESC_CMD_BUFFER_LEN; i++)
    {
        buf[i * 4] = (data[0] & (0x01 << (ESC_CMD_BUFFER_LEN - i - 1))) ? ESC_BIT_1 : ESC_BIT_0;
        buf[i * 4 + 1] = (data[1] & (0x01 << (ESC_CMD_BUFFER_LEN - i - 1))) ? ESC_BIT_1 : ESC_BIT_0;
        buf[i * 4 + 2] = (data[2] & (0x01 << (ESC_CMD_BUFFER_LEN - i - 1))) ? ESC_BIT_1 : ESC_BIT_0;
        buf[i * 4 + 3] = (data[3] & (0x01 << (ESC_CMD_BUFFER_LEN - i - 1))) ? ESC_BIT_1 : ESC_BIT_0;
    }

    ret = write(buf, 4 * 18 * 2);

    return 0;
}

unsigned short dshot::add_checksum(unsigned short value)
{
    uint16_t packet_telemetry = (value << 1);
    uint16_t csum = 0;
    uint16_t csum_data = packet_telemetry;

    for (int i = 0; i < 3; i++)
    {
        csum ^= csum_data;
        csum_data >>= 4;
    }
    csum &= 0xf;

    return (packet_telemetry << 4) | csum;
}
